#include "robot.h"
#include <iostream>

#define up upcell().x][upcell().y
#define down downcell().x][downcell().y
#define left leftcell().x][leftcell().y
#define right rightcell().x][rightcell().y
#define curpos pos.x][pos.y

Robot::Robot(void)
{
}

Robot::~Robot(void)
{
}

void Robot::init(istream& istr)
{
	istr>>pos.x;
	istr>>pos.y;
	istr>>face;
	istr>>sourceMap;
	//cout<<sourceMap.size().x<<" "<<sourceMap.size().y<<endl;//test
	//cout<<sourceMap<<endl;//test
}

Pair_xy Robot::getPos()
{
	return pos;
}

//Doc thong tin o dang dat chan
void Robot::readSourceMap()
{
	string temp=sourceMap(pos.x,pos.y);
	MapCell& k=iMap[curpos];
	k.setVisited(true);
	cout<<"Visit a cell"<<endl;//test

	/*if(temp.find("_")!=string::npos) 
		k.setNothing(1);
	else k.setNothing(0);*/

	if(temp.find("p")!=string::npos) 
		k.setPit(1);
	else k.setPit(0);

	if(temp.find("m")!=string::npos) 
		k.setMummy(1);
	else k.setMummy(0);

	if(temp.find("g")!=string::npos) 
		k.setGold(1);
	else k.setGold(0);

	if(temp.find("b")!=string::npos) 
		k.setBreeze(1);
	else k.setBreeze(0);

	if(temp.find("s")!=string::npos) 
		k.setStench(1);
	else k.setStench(0);

	if(k.isPit()==0 && k.isMummy()==0)
		k.setOK(1);
	else k.setOK(0);
}

int Robot::getFace()
{
	return face;
}

void Robot::movefw()
{
	Pair_xy ppos=pos;
	cout<<"ppos: "<<ppos.x<<" "<<ppos.y<<endl;//test
	switch(face)
	{
		case UP:
			pos.x--;
			break;
		case DOWN:
			pos.x++;
			break;
		case LEFT:
			pos.y--;
			break;
		case RIGHT:
			pos.y++;
			break;
	}
	cout<<"pos: "<<pos.x<<" "<<pos.y<<endl;//test
	Pre[curpos]=ppos;
	cout<<"movefw"<<endl;//test
}

void Robot::turn(int a)
{
	switch(a)
	{
		case LEFT:
			face--;
			if (face<0)	face=3;
			cout<<"turn left"<<endl;//Test
			break;
		case RIGHT:
			face++;
			if (face>3) face=0;
			cout<<"turn right"<<endl;//Test
			break;
	}
}

void Robot::turn(Pair_xy nextpos)
{
	switch(face)
	{
		case UP:
			if(nextpos.y<pos.y) turn(LEFT);
			if(nextpos.y>pos.y) turn(RIGHT);
			//if(nextpos.x<pos.x);
			if(nextpos.x>pos.x)//Dang sau quay
			{
				turn(RIGHT);
				turn(RIGHT);
			}
			break;
		case DOWN:
			if(nextpos.y<pos.y) turn(RIGHT);
			if(nextpos.y>pos.y) turn(LEFT);
			//if(nextpos.x>pos.x);
			if(nextpos.x<pos.x)//Dang sau quay
			{
				turn(RIGHT);
				turn(RIGHT);
			}
			break;
		case LEFT:
			//if(nextpos.y<pos.y) ;
			if(nextpos.y>pos.y)//Dang sau quay
			{
				turn(RIGHT);
				turn(RIGHT);
			}
			if(nextpos.x<pos.x) turn(RIGHT);
			if(nextpos.x>pos.x) turn(LEFT);			
			break;
		case RIGHT:
			//if(nextpos.y>pos.y) ;
			if(nextpos.y<pos.y)//Dang sau quay
			{
				turn(RIGHT);
				turn(RIGHT);
			}
			if(nextpos.x<pos.x) turn(LEFT);
			if(nextpos.x>pos.x) turn(RIGHT);			
			break;
	}
}

void Robot::grab()
{
	cout<<"grab gold"<<endl;//test
}

void Robot::go()
{
	int end=0;//end=-1:die; end=0:go; end=1:gold
	//Khoi tao ban do do robot dung
	iMap.resize(sourceMap.size().x);
	Pre.resize(sourceMap.size().x);
	for(int i=0;i<sourceMap.size().x;i++)
	{
		iMap[i].resize(sourceMap.size().y);
		Pre[i].resize(sourceMap.size().y);
	}
	cout<<"Khoi tao ban do: OK"<<endl;//test
	//
	end=checkimap();
	//cout<<"end: "<<end<<endl;//test
	//Di tim vang
	while(OKList.size()>0 && end==0)
	{
		if(move())
		{
			cout<<"Move: OK"<<endl;//test
			end=checkimap();
		}
		cout<<OKList.size()<<endl;//test
	}
	//Out put
	switch(end)//test
	{
		case -1://TRUONG HOP KHONG NEN XAY RA
			cout<<"DIE!!!"<<endl;
			break;
		case 0://Bi duong
			cout<<"~:>"<<endl;
			break;
		case 1://Lay duoc vang
			cout<<"GOLD"<<endl;
			break;		
	}
}

bool Robot::move()
{
	bool vu=false,vd=false,vl=false,vr=false;
	if (upcell().x!=-1)
		if(iMap[up].isVisited()==false && iMap[up].isPit()==0 && iMap[up].isMummy()==0)
		{
			turn(upcell());
			movefw();
			return true;
		}
		else if (iMap[up].isVisited()==true) vu=true;
	//cout<<"Reach m 1"<<endl;//test
	if (rightcell().x!=-1)
		if(iMap[right].isVisited()==false && iMap[right].isPit()==0 && iMap[right].isMummy()==0)
		{
			turn(rightcell());
			movefw();
			return true;
		}
		else if (iMap[right].isVisited()==true) vr=true;
	//cout<<"Reach m 2"<<endl;//test
	if (downcell().x!=-1)
		if(iMap[down].isVisited()==false && iMap[down].isPit()==0 && iMap[down].isMummy()==0)
		{
			turn(downcell());
			movefw();
			return true;
		}
		else if (iMap[down].isVisited()==true) vd=true;
	//cout<<"Reach m 3"<<endl;//test
	if (leftcell().x!=-1)
		if(iMap[left].isVisited()==false && iMap[left].isPit()==0 && iMap[left].isMummy()==0)
		{
			turn(leftcell());
			movefw();
			return true;
		}
		else if (iMap[left].isVisited()==true) vl=true;
	//cout<<"Reach m 4"<<endl;//test
	//Neu cell nay khong con duong chua khai pha
	//Loai no ra khoi OKList
	if(vu==true && vd==true && vl==true && vr==true)
	{
		int i=0;
		for(i=0;i<OKList.size();i++)
			if(OKList[i]==pos) break;
		iOKList=OKList.begin();
		iOKList+=i;
		OKList.erase(iOKList);

		if(OKList.size()==0)
			return false;
	}
	//Di cho den khi toi mot o trong OKList
	Pair_xy opos=Pre[curpos];
	do
	{				
		turn(opos);
		movefw();
		opos=Pre[opos.x][opos.y];
	}while(!isInOKList(pos));
	return true;	
}

int Robot::checkimap()//-1 = die; 1 = gold end; 0: keep going
{
	//Lay thong tin o dat chan
	readSourceMap();
	MapCell& k=iMap[curpos];

	cout<<k.isOK()<<endl;//test
	//Co gold
	if(k.isGold()==1)
	{
		grab();
		return 1;
	}
	
	//Co pit, co mummy = die
	//TRUONG HOP TOI TE; KHONG NEN XAY RA
	if(k.isOK()==0)
		return -1;
	else if(k.isOK()==1)
	{
		OKList.push_back(pos);
		cout<<"Push OK success."<<endl;//test
	}

	//Khong co mui
	if(k.isStench()==0)
		clearNearMummy();
	//else if(k.isStench()==1)
	//	clearDiaMummy();
	cout<<"Reach cim 1"<<endl;//test

	//Khong co gio
	if(k.isBreeze()==0)
		clearNearPit();
	//else if(k.isBreeze()==1)
	//	clearDiaPit();
	cout<<"Reach cim 2"<<endl;//test

	return 0;
}

Pair_xy Robot::upcell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.x>0)
	{
		kq.x=pos.x-1;
		kq.y=pos.y;
	}
	return kq;
}

Pair_xy Robot::downcell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.x<sourceMap.size().x-1)
	{
		kq.x=pos.x+1;
		kq.y=pos.y;
	}
	return kq;	
}

Pair_xy Robot::leftcell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.y>0)
	{
		kq.y=pos.y-1;
		kq.x=pos.x;
	}
	return kq;
}

Pair_xy Robot::rightcell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.y<sourceMap.size().y-1)
	{
		kq.y=pos.y+1;
		kq.x=pos.x;
	}
	return kq;
}



Pair_xy Robot::nwcell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.y>0 && pos.x>0)
	{
		kq.y=pos.y--;
		kq.x=pos.x--;
	}
	return kq;
}

Pair_xy Robot::necell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.y<sourceMap.size().y-1 && pos.x>0)
	{
		kq.y=pos.y++;
		kq.x=pos.x--;
	}
	return kq;	
}

Pair_xy Robot::swcell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.y>0 && pos.x<sourceMap.size().x-1)
	{
		kq.y=pos.y--;
		kq.x=pos.x++;
	}
	return kq;	
}

Pair_xy Robot::secell()
{
	Pair_xy kq;
	kq.x=-1;
	kq.y=-1;
	if(pos.y<sourceMap.size().y-1 && pos.x<sourceMap.size().x-1)
	{
		kq.y=pos.y++;
		kq.x=pos.x++;
	}
	return kq;	
}

Pair_xy Robot::nextcell()
{
	switch(face)
	{
		case UP:
			return upcell();
		case DOWN:
			return downcell();
		case LEFT:
			return leftcell();
		case RIGHT:
			return rightcell();
	}
}
//Danh dau cac o lan can la khong co Pit
void Robot::clearNearPit()
{
	if(upcell().x!=-1) 
	{
		iMap[up].setPit(0);
		//if(iMap[up].isMummy()==0) iMap[up].setOK(1);
	}
	if(downcell().x!=-1) 
	{
		iMap[down].setPit(0);
		//if(iMap[down].isMummy()==0) iMap[down].setOK(1);
	}
	if(leftcell().x!=-1) 
	{
		iMap[left].setPit(0);
		//if(iMap[left].isMummy()==0) iMap[left].setOK(1);
	}
	if(rightcell().x!=-1)
	{
		iMap[right].setPit(0);
		//if(iMap[right].isMummy()==0) iMap[right].setOK(1);
	}
}

//Danh dau la cac o lan can khong co Mummy
void Robot::clearNearMummy()
{
	if(upcell().x!=-1)
	{
		iMap[up].setMummy(0);
		//if(iMap[up].isPit()==0) iMap[up].setOK(1);
	}
	if(downcell().x!=-1) 
	{
		iMap[down].setMummy(0);
		//if(iMap[down].isPit()==0) iMap[down].setOK(1);
	}
	if(leftcell().x!=-1)
	{
		iMap[left].setMummy(0);
		//if(iMap[left].isPit()==0) iMap[left].setOK(1);
	}
	if(rightcell().x!=-1)
	{
		iMap[right].setMummy(0);
		//if(iMap[right].isPit()==0) iMap[right].setOK(1);
	}
}
//Kiem o cheo de danh dau cac o lan can la khong co Pit
void Robot::clearDiaPit()
{
	if(nwcell().x!=-1) 
		if(iMap[nwcell().x][nwcell().y].isBreeze()==0)
		{
			iMap[up].setPit(0);
			//if(iMap[up].isMummy()==0) iMap[up].setOK(1);
			iMap[left].setPit(0);
			//if(iMap[left].isMummy()==0) iMap[left].setOK(1);
		}
	if(necell().x!=-1) 
		if(iMap[necell().x][necell().y].isBreeze()==0)
		{
			iMap[up].setPit(0);
			//if(iMap[up].isMummy()==0) iMap[up].setOK(1);
			iMap[right].setPit(0);
			//if(iMap[right].isMummy()==0) iMap[right].setOK(1);
		}
	if(swcell().x!=-1) 
		if(iMap[swcell().x][swcell().y].isBreeze()==0)
		{
			iMap[down].setPit(0);
			//if(iMap[down].isMummy()==0) iMap[down].setOK(1);
			iMap[left].setPit(0);
			//if(iMap[left].isMummy()==0) iMap[left].setOK(1);
		}
	if(secell().x!=-1) 
		if(iMap[secell().x][secell().y].isBreeze()==0)
		{
			iMap[down].setPit(0);
			//if(iMap[down].isMummy()==0) iMap[down].setOK(1);
			iMap[right].setPit(0);
			//if(iMap[right].isMummy()==0) iMap[right].setOK(1);
		}
}
//Kiem o cheo de danh dau cac o lan can la khong co Mummy
void Robot::clearDiaMummy()
{
	if(nwcell().x!=-1) 
		if(iMap[nwcell().x][nwcell().y].isBreeze()==0)
		{
			iMap[up].setMummy(0);
			if(iMap[up].isPit()==0) iMap[up].setOK(1);
			iMap[left].setMummy(0);
			if(iMap[left].isPit()==0) iMap[left].setOK(1);
		}
	if(necell().x!=-1) 
		if(iMap[necell().x][necell().y].isBreeze()==0)
		{
			iMap[up].setMummy(0);
			iMap[right].setMummy(0);
			if(iMap[right].isPit()==0) iMap[right].setOK(1);
		}
	if(swcell().x!=-1) 
		if(iMap[swcell().x][swcell().y].isBreeze()==0)
		{
			iMap[down].setMummy(0);
			if(iMap[down].isPit()==0) iMap[down].setOK(1);
			iMap[left].setMummy(0);
			if(iMap[left].isPit()==0) iMap[left].setOK(1);
		}
	if(secell().x!=-1) 
		if(iMap[secell().x][secell().y].isBreeze()==0)
		{
			iMap[down].setMummy(0);
			if(iMap[down].isPit()==0) iMap[down].setOK(1);
			iMap[right].setMummy(0);
			if(iMap[right].isPit()==0) iMap[right].setOK(1);
		}
}

bool Robot::isInOKList(Pair_xy p)
{
	int i;
	for(i=0;i<OKList.size();i++)
		if(OKList[i]==p) return true;
	return false;
}

bool operator == (Pair_xy a,Pair_xy b)
{
	if(a.x==b.x && a.y==b.y)
		return true;
	return false;
}